//
// Created by jiangbin on 4/22/21.
//
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <iostream>
#include <math.h>
#include <thread>
#include "include/back_wheel.h"
#include "include/front_wheel.h"
#include "include/VideoCapture_line.h"
#include "include/robot_arm.h"
#include "include/i2c_driver.h"
#include "include/ICM20948_driver.h"
#include "include/car_IMU_upload.h"
#include "include/car_odom_upload.h"
#include "include/car_picture_upload.h"

using namespace std;

extern int FLAG;

float get_angle(float x, float y)
{
    float theta = 0;
    theta = atanf(y/x)*180/3.1415926;

    theta = (theta > 0) ? theta : -theta;

    if(x > 0 && y > 0){
        theta = 180 - theta;
    }
    else if(x > 0 && y < 0){
        theta = 180 + theta;
    }
    else if(x < 0 && y < 0){
        theta = 360 - theta;
    }
    return theta;
}

void Image(int argc, char *argv[])
{
    int image = VideoCapture_line(argc,argv);
}

void Motion()
{
    while(!FLAG);
    ahead();
    usleep(500000);
    right(3);
    usleep(500000);
    w_distance(5);
    usleep(500000);
    ahead();
    usleep(500000);
}

void IMU()
{
    float accelX, accelY, accelZ;
    float gyroX, gyroY, gyroZ;
    uint16_t magX, magY, magZ;

    int i2cfd  = open("/dev/i2c-2", O_RDWR);
    if (i2cfd < 0)
        exit(EXIT_FAILURE);

    ioctl(i2cfd, I2C_SLAVE, SLAVE_ADDR);

#if 1
    ICM20948_PowerOn(i2cfd, UB0_PWR_MGMNT_1_DEV_RESET);

    ICM20948_PowerOn(i2cfd, UB0_PWR_MGMT_1_AUTO_CLOCK);

    ICM20948_GyroConfigInit(i2cfd, UB2_GYRO_CONFIG_1_FS_SEL_1000DPS, 
            UB2_GYRO_CONFIG_1_DLPFCFG_12HZ);


    ICM20948_AccelConfigInit(i2cfd, UB2_ACCEL_CONFIG_FS_SEL_8G,
            UB2_ACCEL_CONFIG_DLPF_BANDWIDTH_111HZ);

    ICM20948_EnableI2cMaster(i2cfd);

    ICM20948_AccelGyroOnOrOff(i2cfd, UB0_PWR_MGMT2_ACCEL_GYRO_ON);

    ICM20948_InitMag(i2cfd);

    // float theta, tan = 0.0f;

    while (1) {

        AK09916_GetMag(i2cfd, &magX, &magY, &magZ);
        ICM20948_GetAccel(i2cfd, &accelX, &accelY, &accelZ);
        ICM20948_GetGyro(i2cfd, &gyroX, &gyroY, &gyroZ);

        //magX += -135;
        //magY += 40;

        printf("mag: %d %d %d \n", magX, magY, magZ); // get_angle((float)magX, (float)magY));
        printf("acc: %f %f %f \n", accelX, accelY, accelZ);
        printf("gyr: %f %f %f \n", gyroX, gyroY, gyroZ);

        fflush(stdout);
        usleep(5000);
    }
#endif

    close(i2cfd);
}

int main(int argc,char* argv[]){
    thread t1(Image,argc,argv);
    thread t2(IMU_upload);
    t2.join();
    t1.join();
    return 0;
} 
